/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#ifndef RC_CONTROLS_H
#define RC_CONTROLS_H

#include "rc.h"
#include "zino.h"
#include "fc_sensors.h"

#ifdef __cplusplus
extern "C"
{
#endif

    #define RC_LEVEL_MAX 30
    #define RC_PITCH_MAX 30
    #define RC_YAW_MAX 30
    #define RC_THROTTLE_MAX 100
    #define RC_SAVED_FLAG 0XAB

    typedef enum rc_alias
    {
        ROLL = 0,
        PITCH,
        YAW,
        THROTTLE,
        AUX1,
        AUX2,
        AUX3,
        AUX4,
        AUX5,
        AUX6,
        AUX7,
        AUX8,
        AUX9,
        AUX10,
        AUX11,
        AUX12
    } rc_alias_e;
    typedef enum
    {
        FD_ROLL = 0,
        FD_PITCH,
        FD_YAW
    } flight_dynamics_index_t;
    typedef struct rcControlsConfig_s
    {
        int16_t max_check;
        int16_t min_check;
        int16_t mid_check;
        uint8_t deadband;             // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
        uint8_t yaw_deadband;         // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
        uint8_t alt_hold_deadband;    // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
        uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
        int16_t rcData[4];            // raw data
        int16_t rcCommand[4];         // processed data
        uint8_t dt_min;
        float RcDeflection[4];     // sacle rcCommand to -1.0 to 1.0
        float RcDeflectionAbs[4];  // sacle rcCommand to 0 to 1.0
        float setpointRate[4];     // scale to -1998 to 1988, here [-998,998]
        bool yaw_control_reversed; // invert control direction of yaw
    } rcControlsConfig_t;
    struct rc_control_max
    {
        uint8_t flag;
        uint8_t level;
        uint8_t yaw;
        uint8_t throttle;
    };


    // 期望值数据
    typedef struct
    {
        attitude_t angle;   // 角度
        attitude_t rate;   // 角度
        Axis_3_float_t vel; // 速度
        Axis_3_float_t pos; // 位置
        bool manual[3]; // 手动控制
    } fc_setpoint_t;
    extern fc_setpoint_t fc_setpoint;

    ZINO_CONFIG_DECLARE(struct rc_control_max, rc_control_max);

#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL))
#define PIT_LO (1 << (2 * PITCH))
#define PIT_CE (3 << (2 * PITCH))
#define PIT_HI (2 << (2 * PITCH))
#define YAW_LO (1 << (2 * YAW))
#define YAW_CE (3 << (2 * YAW))
#define YAW_HI (2 << (2 * YAW))
#define THR_LO (1 << (2 * THROTTLE))
#define THR_CE (3 << (2 * THROTTLE))
#define THR_HI (2 << (2 * THROTTLE))

#define GET_DIRECTION(isReversed) ((isReversed) ? -1 : 1)

    void rcControlsUpate(void);
    float getRcDeflection(uint8_t axis);
    float getRcDeflectionAbs(uint8_t axis);
    float getSetpointRate(uint8_t axis);
    void rc_control_max_init(void);
    void fc_landing_handle(rcControlsConfig_t *rc);
    void fc_takingoff_handle(rcControlsConfig_t *rc);
    extern rcControlsConfig_t rc_controler;

#ifdef __cplusplus
}
#endif

#endif /* RC_CONTROLS_H */
